package de.fhwf.mixedreality.client2008;

import de.fhwf.mixedreality.client.IMovements;
import de.fhwf.mixedreality.client.Vector;

public class Movements implements IMovements {
	private PositionsStorage mPositions;
	private CommandInterface mCmd;

	public Movements(PositionsStorage positions, CommandInterface command) {
		mPositions = positions;
		mCmd = command;
	}

	/* (non-Javadoc)
	 * @see de.fhwf.mixedreality.client2008.IMovements#goTo(de.fhwf.mixedreality.client.Vector)
	 */
	public void goTo(Vector target) {
		// Adjust the angle to be between -180 and 180 degrees.
		target.boundAngle();

		// If the angle error is bigger than 90 degrees let's just turn
		if (target.getAngle() > Math.PI / 2) {
			turnRight();
		} else if (target.getAngle() < -Math.PI / 2) {
			turnLeft();
		} else if (target.getLength() > 300) { // Otherwise let's check the distance
			// We are still far, let's not care much about being completely aligned.
			if (target.getAngle() > (20.0 * Math.PI / 180.0)) {
				curveRight();
			} else if (target.getAngle() < (-20.0 * Math.PI / 180.0)) {
				curveLeft();
			} else if (target.getLength() > 600) {
				fastForward();
			} else {
				goStraight(3);
			}
		} else if (target.getLength() > 55) {
			// Okay we are closer, let's narrow the angle
			if (target.getAngle() > (10.0 * Math.PI / 180.0)) {
				curveRight();
			} else if (target.getAngle() < (-10.0 * Math.PI / 180.0)) {
				curveLeft();
			} else {
				// Angle seems OK, let's keep going straight ahead
				goStraight(1);
			}
		} else if (target.getLength() > 20) {
			// Okay we are closer, let's narrow the angle
			if (target.getAngle() > (5.0 * Math.PI / 180.0)) {
				setVelocity(1, 0);
			} else if (target.getAngle() < (-5.0 * Math.PI / 180.0)) {
				setVelocity(0, 1);
			} else {
				// Angle seems OK, let's keep going straight ahead
				goStraight(1);
			}
		} else {
			// Good enough! We stop here.
			stop();
		}
	}

	/* (non-Javadoc)
	 * @see de.fhwf.mixedreality.client2008.IMovements#setVelocity(int, int)
	 */
	public void setVelocity(int left, int right) {
		mCmd.setVelocity(left, right);
	}

	/**
	 * Directly set the velocity of the wheels.
	 * The allowed range for the values depends on the robot version used:
	 * 2008: -130 .. 0 .. 130 (mm/sec)
	 */
	public void setRawSpeed(int left, int right) {
		mCmd.setMMPS(left, right);
	}
	
	/** Send raw command string (four characters) */
	@Deprecated
	public void rawCommand(String cmd) {
		mCmd.setRaw(cmd);
	}

	/* (non-Javadoc)
	 * @see de.fhwf.mixedreality.client2008.IMovements#goStraight(int)
	 */
	public void goStraight(int speed) {
		setVelocity(speed, speed);
	}

	/* (non-Javadoc)
	 * @see de.fhwf.mixedreality.client2008.IMovements#stop()
	 */
	public void stop() {
		setVelocity(0, 0);
	}

	/* (non-Javadoc)
	 * @see de.fhwf.mixedreality.client2008.IMovements#turnLeft()
	 */
	public void turnLeft() {
		setVelocity(-2, 2);
	}

	/* (non-Javadoc)
	 * @see de.fhwf.mixedreality.client2008.IMovements#turnRight()
	 */
	public void turnRight() {
		setVelocity(2, -2);
	}

	/* (non-Javadoc)
	 * @see de.fhwf.mixedreality.client2008.IMovements#curveLeft()
	 */
	public void curveLeft() {
		setVelocity(1, 2);
	}

	/* (non-Javadoc)
	 * @see de.fhwf.mixedreality.client2008.IMovements#curveRight()
	 */
	public void curveRight() {
		setVelocity(2, 1);
	}
	
	/* (non-Javadoc)
	 * @see de.fhwf.mixedreality.client2008.IMovements#fastForward()
	 */
	public void fastForward() {
		setVelocity(4, 4);
	}

	/* (non-Javadoc)
	 * @see de.fhwf.mixedreality.client2008.IMovements#fastBackward()
	 */
	public void fastBackward() {
		setVelocity(-4, -4);
	}

	/* (non-Javadoc)
	 * @see de.fhwf.mixedreality.client2008.IMovements#chase(de.fhwf.mixedreality.client.Vector)
	 */
	public void chase(Vector target) {
		goTo(target);
	}

	/* (non-Javadoc)
	 * @see de.fhwf.mixedreality.client2008.IMovements#escapeFrom(de.fhwf.mixedreality.client.Vector)
	 */
	public void escapeFrom(Vector enemy) {
		goTo(new Vector(enemy.getAngle() + Math.PI, enemy.getLength()));
	}

	/* (non-Javadoc)
	 * @see de.fhwf.mixedreality.client2008.IMovements#kick(double, double)
	 */
	public void kick(double angle, double strength) {
		mCmd.setKick(angle, strength);
	}

	/* (non-Javadoc)
	 * @see de.fhwf.mixedreality.client2008.IMovements#canKick()
	 */
	public boolean canKick() {
		Vector t = mPositions.ball();
		if (t.getLength() <= 30) {
			return true;
		} else {
			return false;
		}
	}
	
	public void serverGoTo(Vector target) {
		mCmd.setGoto(target.getLength(), target.getAngle());
	}	
}